#include <FastSerial.h>
#include <Servo.h>
#include <WProgram.h>
//Serial
#define CAMERAPORT Serial2
#define IMUPORT Serial1
#define DEBUGPORT Serial
#define BUFFERSIZE 32
FastSerialPort0(DEBUGPORT);
FastSerialPort1(IMUPORT);
FastSerialPort2(CAMERAPORT);

//Servos
Servo ROLSERVO;
#define ROLPIN 22
#define ROLMAX 4000 //100 times degrees
#define ROLMIN -4000//100 times degrees
#define ROLCMDNEU 1440
#define ROLCMDMAX 1980//microseconds
#define ROLCMDMIN 900//microseconds
Servo PITSERVO;
#define PITPIN 23
#define PITMAX 2000//100 times degrees
#define PITMIN -2000//100 times degrees
#define PITCMDNEU 1142
#define PITCMDMAX 1384//microseconds
#define PITCMDMIN 900//microseconds
#define PITCMDRETRACT 2100
Servo TILSERVO;
#define TILPIN 38
#define TILMAX 4500//100 times degrees
#define TILMIN -4500//100 times degrees
#define TILCMDNEU 1500
#define TILCMDMAX 2100//microseconds
#define TILCMDMIN 900//microsecond
Servo PANSERVO;
#define PANPIN 34
#define PANMAX 9000 //100 times degree
#define PANMIN -9000 //100 times degrees
#define PANCMDNEU 1500
#define PANCMDMAX 1800
#define PANCMDMIN 1200
#define PANDEAD 2
#define PHOMAX 70 //1/100 of PAMAX
#define PHOMIN -70 //1/100 of PAMIN

//interrupts
#define INT4PIN 2
#define INT5PIN 3
#define INT1PIN 20
#define INT0PIN 21
#define ICP4PIN 49

//other
#define IMUPIN 8 //pin for groundstart signal
#define LEDPIN 13

//global interrupt variables
volatile unsigned int timer1INT0;
volatile unsigned int timer2INT0;
volatile byte flagINT0;
volatile unsigned int timer1INT1;
volatile unsigned int timer2INT1;
volatile byte flagINT1;
volatile int photopos=0;
volatile byte dir=0;
volatile byte flagICP4=0;

//int0 for pan signal
ISR(INT0_vect)
{
  if (EICRA & B00000001) {//if we captured rising
    EICRA = EICRA ^ B00000001;//falling edge
    timer1INT0=TCNT4;
    flagINT0=0;
  }
  else {
    EICRA = EICRA ^ B00000001;//rising edge
    timer2INT0=TCNT4;
    flagINT0=1;
  }
}

//int1 for tilt signal
ISR(INT1_vect)
{
  if (EICRA & B00000100) {//if we captured rising
    EICRA= EICRA ^ B00000100;//falling edge
    timer1INT1=TCNT4;
    flagINT1=0;
  }
  else {
    EICRA=EICRA ^ B00000100;//rising edge
    timer2INT1=TCNT4;
    flagINT1=1;
  }
}

//int4 for photo1
//ISR(INT4_vect)
//{
//  if (dir)
//    photopos++;
//  else
//    photopos--;
//}

//int5 for photo 2 detects both edges!
ISR(INT5_vect)
{
  if (EICRB & B00000100)//if we captured rising
    EICRB= EICRB ^ B00000100;//falling edge
  else
    EICRB=EICRB ^ B00000100;//rising edge
  
  if (dir)
    photopos++;
  else
    photopos--;
}

//ICP4 for hall effect
//ISR(TIMER4_CAPT_vect)
//{
//  if (!digitalRead(ICP4PIN)) {// magnet
//    TCCR4B|=B01000000;//set rising edge
//    flagICP4=1;
//    photopos=0;
//  }
//  else { //no magnet
//    TCCR4B&=B10111111;//set falling edge
//    photopos=photopos/2;
//  }
//}

//ICP4 for photo1
ISR(TIMER4_CAPT_vect)
{
  if (TCCR4B & B01000000)//if we captured rising
    TCCR4B=TCCR4B ^ B01000000;//falling edge
  else
    TCCR4B=TCCR4B ^ B01000000;//rising edge
  
  if (dir)
    photopos++;
  else
    photopos--;
}

//receive function for debug terminal
byte DEBUGreceive(char* buffer) {
  if (DEBUGPORT.available() > 0) {
    byte i;
    for (i=0;i<BUFFERSIZE;i++) {
      while (DEBUGPORT.available() == 0);
      buffer[i]=DEBUGPORT.read();
      if (buffer[i]=='!' || buffer[i]=='?') {//EOL
        return (i+1);
      }
    }
  }
  return 0;
}

//receive function for IMU
byte IMUreceive(byte* buffer) {
  if (IMUPORT.available() >= 6) { //4prefix+size+ID
    if (IMUPORT.read()=='D') { //PRE1
      if (IMUPORT.read()=='I') { //PRE2
        if (IMUPORT.read()=='Y') { //PRE3
          if (IMUPORT.read()=='d') { //PRE4
            buffer[0]=IMUPORT.read(); //payload size
            if (buffer[0]>BUFFERSIZE-2){//if detected size is too large for buffer
              IMUPORT.flush();
              return 0;
            }
            buffer[1]=IMUPORT.read(); //Message ID
            while (IMUPORT.available()<buffer[0]);
            byte i;
            for (i = 2; i < buffer[0]+2 ; i++)
              buffer[i] = IMUPORT.read();
            IMUPORT.flush(); //ignore cksum
            return 1; //message available
          }
        }
      }
    }
  }
  return 0; //no message available
}

//helper function for IMU data packets
int bytes2int(byte* buffer)
{
  union bytes2int {
    byte b[2];
    int i;
  } 
  bytes2int;
  bytes2int.b[0]=buffer[0];
  bytes2int.b[1]=buffer[1];
  return(bytes2int.i);
}

//helper function for IMU data packets
long bytes2long(byte* buffer)
{
  union bytes2long {
    byte b[4];
    long l;
  } 
  bytes2long;
  bytes2long.b[0]=buffer[0];
  bytes2long.b[1]=buffer[1];
  bytes2long.b[2]=buffer[2];
  bytes2long.b[3]=buffer[3];
  return(bytes2long.l);
}

